// Copyright 2016 Proyectos y Sistemas de Mantenimiento SL (eProsima).
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// ------------------------------------------------------------------
// Modification history:
// feature: agent for vbs
// ------------------------------------------------------------------

#include <functional>

#include <edds/dds/core/policy/QosPolicies.hpp>
#include <edds/dds/core/policy/ParameterList.hpp>
#include <edds/dds/core/policy/ParameterSerializer.hpp>

namespace evbs {
namespace edds {
namespace dds {

bool ParameterList::writeEncapsulationToCDRMsg(ertps::rtps::CDRMessage_t* const msg) {
    bool valid = ertps::rtps::CDRMessage::addOctet(msg, 0);
    valid =
        ertps::rtps::CDRMessage::addOctet(msg, static_cast<ertps::rtps::octet>(PL_CDR_LE - msg->msg_endian)) && valid;
    valid = ertps::rtps::CDRMessage::addUInt16(msg, 0U) && valid;
    return valid;
}

bool ParameterList::writeUint32ToCDRMsg(ertps::rtps::CDRMessage_t* const msg, ParameterId_t type, uint16_t length,
                                        uint32_t value) {
    bool valid = ertps::rtps::CDRMessage::addUInt16(msg, type);
    valid = ertps::rtps::CDRMessage::addUInt16(msg, length) && valid;
    return ertps::rtps::CDRMessage::addUInt32(msg, value) && valid;
}

bool ParameterList::updateCacheChangeFromInlineQos(vbs::common::CacheChange_t& change,
                                                   ertps::rtps::CDRMessage_t* const msg, uint32_t& qos_size) {
    auto parameter_process = [&](ertps::rtps::CDRMessage_t* const cdrmsg, const ParameterId_t pid,
                                 const uint16_t plength) {
        switch (pid) {
            case PID_KEY_HASH: {
                ParameterKey_t p(pid, plength);
                if (!edds::dds::ParameterSerializer<ParameterKey_t>::read_from_cdr_message(p, cdrmsg, plength)) {
                    elogError(RTPS_MSG_IN, RetCode_t::RETCODE_ERROR,
                              "SubMessage Data ERROR, Inline Qos ParameterList PID_KEY_HASH error");
                    return false;
                }

                change.instanceHandle = p.key;
                break;
            }

            case PID_RELATED_SAMPLE_IDENTITY: {
                if (plength >= 24U) {
                    ParameterSampleIdentity_t p(pid, plength);
                    if (!edds::dds::ParameterSerializer<ParameterSampleIdentity_t>::read_from_cdr_message(p, cdrmsg,
                                                                                                          plength)) {
                        elogError(RTPS_MSG_IN, RetCode_t::RETCODE_ERROR,
                                  "SubMessage Data ERROR, Inline Qos ParameterList PID_RELATED_SAMPLE_IDENTITY error");
                        return false;
                    }

                    (void)change.write_params.sample_identity(p.sample_id);
                }
                break;
            }

            case PID_STATUS_INFO: {
                ParameterStatusInfo_t p(pid, plength);
                if (!edds::dds::ParameterSerializer<ParameterStatusInfo_t>::read_from_cdr_message(p, cdrmsg, plength)) {
                    elogError(RTPS_MSG_IN, RetCode_t::RETCODE_ERROR,
                              "SubMessage Data ERROR, Inline Qos ParameterList PID_STATUS_INFO error");
                    return false;
                }

                std::map<uint8_t, vbs::common::ChangeKind_t> kind_maps = {
                    {1, vbs::common::ChangeKind_t::NOT_ALIVE_DISPOSED},
                    {2, vbs::common::ChangeKind_t::NOT_ALIVE_UNREGISTERED},
                    {3, vbs::common::ChangeKind_t::NOT_ALIVE_DISPOSED_UNREGISTERED},
                };
                auto it = kind_maps.find(p.status);
                if (it != kind_maps.end()) {
                    change.kind = it->second;
                }
                break;
            }

            case PID_TOPIC_ID: {
                uint32_t topic_id = 0;
                if (plength == static_cast<uint16_t>(sizeof(topic_id))) {
                    if (!ertps::rtps::CDRMessage::readUInt32(cdrmsg, &topic_id)) {
                        elogError(RTPS_MSG_IN, RetCode_t::RETCODE_ERROR,
                                  "SubMessage Data ERROR, Inline Qos ParameterList PID_TOPIC_ID error");
                        return false;
                    }
                    change.topic_id = topic_id;
                }
                break;
            }

            default:
                break;
        }

        return true;
    };

    return readParameterListfromCDRMsg(*msg, parameter_process, false, qos_size);
}

bool ParameterList::read_guid_from_cdr_msg(ertps::rtps::CDRMessage_t& msg, const uint16_t search_pid,
                                           ertps::rtps::GUID_t& guid) {
    while (msg.pos < msg.length) {
        bool valid;
        uint16_t pid;
        uint16_t plength;

        valid = ertps::rtps::CDRMessage::readUInt16(&msg, &pid);
        valid = ertps::rtps::CDRMessage::readUInt16(&msg, &plength) && valid;
        if ((pid == PID_SENTINEL) || (!valid)) {
            break;
        }
        if (pid == PID_KEY_HASH) {
            valid = ertps::rtps::CDRMessage::readData(&msg, guid.guidPrefix.value, ertps::rtps::GuidPrefix_t::size);
            valid =
                ertps::rtps::CDRMessage::readData(&msg, guid.entityId.value, ertps::rtps::EntityId_t::size) && valid;
            return valid;
        }
        if (pid == search_pid) {
            valid = ertps::rtps::CDRMessage::readData(&msg, guid.guidPrefix.value, ertps::rtps::GuidPrefix_t::size);
            valid =
                ertps::rtps::CDRMessage::readData(&msg, guid.entityId.value, ertps::rtps::EntityId_t::size) && valid;
            return valid;
        }
        msg.pos += (plength + 3U) & ~3U;
    }
    return false;
}

bool ParameterList::readInstanceHandleFromCDRMsg(vbs::common::CacheChange_t* const change, const uint16_t search_pid) {
    assert(change != nullptr);

    // Only process data when change does not already have a handle
    if (change->instanceHandle.isDefined()) {
        return true;
    }

    // Use a temporary wraping message
    ertps::rtps::CDRMessage_t msg(change->serializedPayload);

    // Read encapsulation
    msg.pos += 1U;
    ertps::rtps::octet encapsulation = 0U;
    (void)ertps::rtps::CDRMessage::readOctet(&msg, &encapsulation);
    if (encapsulation == PL_CDR_BE) {
        msg.msg_endian = ertps::rtps::Endianness_t::BIGEND;
    } else if (encapsulation == PL_CDR_LE) {
        msg.msg_endian = ertps::rtps::Endianness_t::LITTLEEND;
    } else {
        return false;
    }

    change->serializedPayload.encapsulation = static_cast<uint16_t>(encapsulation);

    // Skip encapsulation options
    // msg.pos += 2U;

    // Read encapsulationCompletion
    msg.pos += 1U;
    ertps::rtps::octet encapsulationCompletion = 0U;
    (void)ertps::rtps::CDRMessage::readOctet(&msg, &encapsulationCompletion);
    change->serializedPayload.encapsulationCompletion = static_cast<uint16_t>(encapsulationCompletion);

    uint16_t pid = 0U;
    uint16_t plength = 0U;
    while (msg.pos < msg.length) {
        bool valid;
        valid = ertps::rtps::CDRMessage::readUInt16(&msg, static_cast<uint16_t*>(&pid));
        valid = ertps::rtps::CDRMessage::readUInt16(&msg, &plength) && valid;
        if ((pid == PID_SENTINEL) || (!valid)) {
            break;
        }
        if (pid == PID_KEY_HASH) {
            valid = ertps::rtps::CDRMessage::readData(&msg, change->instanceHandle.value, 16U);
            return valid;
        }
        if (pid == search_pid) {
            valid = ertps::rtps::CDRMessage::readData(&msg, change->instanceHandle.value, 16U);
            return valid;
        }
        msg.pos += (plength + 3U) & ~3U;
    }
    return false;
}

}  // namespace dds
}  // namespace edds
}  // namespace evbs
